/***************************************************************************
 创建者: 华磊
 开始时间: 2019.5.24
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************/
#include "path_3bezier.h"
#include <QDebug>

#define D_ALLOW_LENGTH 0.0001

namespace KDL {

Path_3Bezier::Path_3Bezier(Vector startPoint,Vector startPoint_orient,
                           Vector endPoint,Vector endPoint_orient)
{
    qDebug()<<"startPoint"<<startPoint.data[0]<<startPoint.data[1]
           <<"startPoint_orient"<<startPoint_orient.data[0]<<startPoint_orient.data[1]
             <<"endPoint"<<endPoint.data[0]<<endPoint.data[1]
               <<"endPoint_orient"<<endPoint_orient.data[0]<<endPoint_orient.data[1];
    V_base_start=startPoint;
    V_base_end=endPoint;
    V_base_start_orient=startPoint_orient;
    V_base_end_orient=endPoint_orient;
    V_start_end = V_base_end - V_base_start;
    double dist = V_start_end.Normalize();


    pathlength=getBezier_length(startPoint,startPoint_orient,endPoint_orient,endPoint,30);
    scalelin=pathlength;

}

Path_3Bezier::~Path_3Bezier()
{

}

double Path_3Bezier::LengthToS(double length)
{
    double tmpS=length/scalelin;
    qDebug()<<"LengthToS length"<<length<<"tmpS"<<tmpS;//not used
    return tmpS;
}

double Path_3Bezier::PathLength()
{
    if(D_ALLOW_LENGTH>pathlength)
    {
        return 0;
    }
    return pathlength;
}

Frame Path_3Bezier::Pos(double s) const
{

//   return Frame(orient->Pos(s*scalerot),V_base_start + V_start_end*s*scalelin );
    double tmpS;
    if(D_ALLOW_LENGTH>pathlength)
    {
        tmpS=0;
    }
    else
    {
        tmpS=s/scalelin;
    }

    Frame tmpFrame(bezierPoint(tmpS));
//    qDebug()<<"Path_3Bezier::Pos s"<<s<<"Frame"<<tmpFrame.p.data[0]<<tmpFrame.p.data[1]<<"pathlength"<<pathlength;
    return tmpFrame;
}

Twist Path_3Bezier::Vel(double s, double sd) const
{
    Twist tmpTwist;
    tmpTwist.vel.data[0]=sd*scalelin;
    return tmpTwist;
}

Twist Path_3Bezier::Acc(double s, double sd, double sdd) const
{

    Twist tmpTwist;
    tmpTwist.vel.data[0]=sdd*scalelin;
    return tmpTwist;
}

Path *Path_3Bezier::Clone()
{
//    return new Path_3Bezier();
}

void Path_3Bezier::Write(std::ostream &os)
{
    os << "SPLINE[ ";
//	os << "  " << Frame(orient->Pos(0),V_base_start) << std::endl;
//	os << "  " << Frame(orient->Pos(pathlength*scalerot),V_base_end) << std::endl;
//	os << "  ";orient->Write(os);
//	os << "  " << eqradius;
    os << "]"  << std::endl;
}



Vector Path_3Bezier::bezierPoint(double t) const
{
    double u = 1 - t;
    double tt = t * t;
    double uu = u * u;
    double uuu = uu * u;
    double ttt = tt * t;

    Vector p;
    p.data[0] = uuu * V_base_start.data[0];
    p.data[1] = uuu * V_base_start.data[1];

    p.data[0] += 3 * uu * t * V_base_start_orient.data[0];
    p.data[1] += 3 * uu * t * V_base_start_orient.data[1];

    p.data[0] += 3 * u * tt * V_base_end_orient.data[0];
    p.data[1] += 3 * u * tt * V_base_end_orient.data[1];

    p.data[0] += ttt * V_base_end.data[0];
    p.data[1] += ttt * V_base_end.data[1];

    return p;
}

Vector Path_3Bezier::bezierPoint(double t, Vector p0, Vector p1, Vector p2, Vector p3)
{
    double u = 1 - t;
    double tt = t * t;
    double uu = u * u;
    double uuu = uu * u;
    double ttt = tt * t;

    Vector p;
    p.data[0] = uuu * p0.data[0];
    p.data[1] = uuu * p0.data[1];

    p.data[0] += 3 * uu * t * p1.data[0];
    p.data[1] += 3 * uu * t * p1.data[1];

    p.data[0] += 3 * u * tt * p2.data[0];
    p.data[1] += 3 * u * tt * p2.data[1];

    p.data[0] += ttt * p3.data[0];
    p.data[1] += ttt * p3.data[1];

    return p;
}

double Path_3Bezier::getBezier_length(Vector p0, Vector p1, Vector p2, Vector p3, int pointCount)
{
    //取点 默认 30个
//    pointCount = 30;
    double length = 0.0;
    Vector lastPoint = bezierPoint(0.0 / (double)pointCount, p0, p1, p2, p3);
    for (int i = 1; i <= pointCount; i++)
    {
        Vector point = bezierPoint((double)i / (double)pointCount, p0, p1, p2, p3);
        length += sqrt((point.data[0] - lastPoint.data[0]) * (point.data[0] - lastPoint.data[0]) + (point.data[1] - lastPoint.data[1]) * (point.data[1] - lastPoint.data[1]));
        lastPoint = point;
    }
    return length;
}

//int Path_3Bezier::GetNrOfSegments()
//{

//}

//Path *Path_3Bezier::GetSegment(int i)
//{

//}

//double Path_3Bezier::GetLengthToEndOfSegment(int i)
//{

//}

//void Path_3Bezier::GetCurrentSegmentLocation(double s, int &segment_number, double &inner_s)
//{

//}


//定义一个三阶贝塞尔曲线函数，t的范围在[0,1]；
//输入P0, P1, P2, P3，分别为起始点，控制1点，控制2点，终止点；
//https://blog.csdn.net/u011643833/article/details/78540554
POINT Path_3Bezier::bezierPoint(double t, POINT p0, POINT p1, POINT p2, POINT p3)
{
    double u = 1 - t;
    double tt = t * t;
    double uu = u * u;
    double uuu = uu * u;
    double ttt = tt * t;

    POINT p;
    p.x = uuu * p0.x;
    p.y = uuu * p0.y;

    p.x += 3 * uu * t * p1.x;
    p.y += 3 * uu * t * p1.y;

    p.x += 3 * u * tt * p2.x;
    p.y += 3 * u * tt * p2.y;

    p.x += ttt * p3.x;
    p.y += ttt * p3.y;

    return p;
}


double Path_3Bezier::getBezier_length(POINT p0, POINT p1, POINT p2, POINT p3, int pointCount)
{
    //取点 默认 30个
    pointCount = 30;
    double length = 0.0;
    POINT lastPoint = bezierPoint(0.0 / (double)pointCount, p0, p1, p2, p3);
    for (int i = 1; i <= pointCount; i++)
    {
        POINT point = bezierPoint((double)i / (double)pointCount, p0, p1, p2, p3);
        length += sqrt((point.x - lastPoint.x) * (point.x - lastPoint.x) + (point.y - lastPoint.y) * (point.y - lastPoint.y));
        lastPoint = point;
    }
    return length;
}



}
